"""  
    需求：以某个固定频率发送文本“hello world!”，文本后缀编号，每发送一条消息，编号递增1。
    步骤：
        1.导包；
        2.初始化 ROS2 客户端；
        3.定义节点类；
            3-1.创建发布方；
            3-2.创建定时器；
            3-3.组织消息并发布。
        4.调用spin函数，并传入节点对象；
        5.释放资源。
"""
# 1.导包；
import rclpy
from rclpy.node import Node
from std_msgs.msg import String #topic1
from std_msgs.msg import Float32 #topic1
from std_msgs.msg import Header
from geometry_msgs.msg import PoseWithCovariance #odom的pose的格式
from geometry_msgs.msg import PoseStamped #ap/pose的格式
from geometry_msgs.msg import TwistWithCovariance #odom的twist的格式 
from geometry_msgs.msg import TwistStamped #ap/twist的格式
from nav_msgs.msg import Odometry #ROS的odom的格式
from rclpy.qos import QoSProfile, ReliabilityPolicy # QOS

# 3.定义节点类；
class Talker(Node):
    def __init__(self):
        super().__init__("ap_pose_Node")#创建节点名字
        self.get_logger().info("发布方创建了ap/pose和ap/twist（.py）!")#简单点，获取一个日志对象
        #设置计数器
        self.count = 0
        # 设置为不可靠（BEST_EFFORT）的QoS策略
        qos_profile = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT  # 设置为不可靠性
        )
        # 3.定义节点类；
            # 3-1.创建发布方；
        """ 
            参数：
               1.消息类型
               2.话题名称
               3.QOS，队列长度
               返回值：发布对象
        """
        self.publisher1 = self.create_publisher(String,"PUB1",10)   #=左为自定义名称，=右为调用函数
        self.publisher2 = self.create_publisher(Float32,"PUB2",10)
        self.publisher3 = self.create_publisher(PoseStamped,"ap/pose/filtered",qos_profile)
        self.publisher4 = self.create_publisher(TwistStamped,"ap/twist/filtered",qos_profile)
        
            # 3-2.创建定时器；
        self.timer = self.create_timer(2.0,self.on_timer)
            
    # 3-3.组织消息并发布。
    def on_timer(self):
        message = String()
        message1 = Float32()
        message2 = PoseStamped()
        message3 = TwistStamped()

        message.data = "hell0_py" + str(self.count)
        message1.data = 1.0 + self.count

        message2.header = Header()
        message2.header.stamp = self.get_clock().now().to_msg()
        message2.header.frame_id = "base_link"
        message2.pose.position.x = 1.0 +self.count
        message2.pose.position.y = 2.0 
        message2.pose.position.z = 0.0
        message2.pose.orientation.x = 0.0 
        message2.pose.orientation.y = 0.0 
        message2.pose.orientation.z = 0.0
        message2.pose.orientation.w = 1.0

        message3.header = Header()
        message3.header.stamp = self.get_clock().now().to_msg()
        message3.header.frame_id = "base_link"
        message3.twist.linear.x = 10.0 + self.count
        message3.twist.linear.y = 20.0 
        message3.twist.linear.z = 30.0
        message3.twist.angular.x = 11.0
        message3.twist.angular.y = 13.0
        message3.twist.angular.z = 12.0



        self.publisher1.publish(message)
        self.publisher2.publish(message1)
        self.publisher3.publish(message2)
        self.publisher4.publish(message3)

        self.count += 1
        self.get_logger().info("发布：%s" % message.data)
        self.get_logger().info("发布1：%d" % message1.data)
        self.get_logger().info("发布header:sec：%d" % message2.header.stamp.sec)
        self.get_logger().info("发布header:nanosec：%d" % message2.header.stamp.nanosec)#后加的
        self.get_logger().info("发布header:frameid：%s" % message2.header.frame_id)
        self.get_logger().info("发布pose：x:%d , y:%d ,z:%f" % (message2.pose.position.x, message2.pose.position.y, message2.pose.position.z))
        self.get_logger().info("发布orien：x:%d , y:%d ,z:%f w:%d" % (message2.pose.orientation.x, message2.pose.orientation.y, message2.pose.orientation.z, message2.pose.orientation.w))


def main():
    # 2.初始化 ROS2 客户端；
    rclpy.init()
    # 4.调用spin函数，并传入节点对象；
    rclpy.spin(Talker())
    # 5.释放资源。
    rclpy.shutdown()

if __name__ == '__main__':
    main()